
// 贝塞尔曲线

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>

#include <iostream>
#include <random>
using namespace std;

class BezierCurve{
    public:
        BezierCurve();
        ~BezierCurve() {};
        // 发布点云数据
        void PublishPointCloud2(vector<geometry_msgs::Point>);
        // 发布直线数据
        void PublishLine(vector<geometry_msgs::Point> points);
        // 
        int Comb(int n, int r);
        // 计算贝塞尔曲线关键点
        geometry_msgs::Point BezierPoint(double t, vector<geometry_msgs::Point> points);
    
    private:
        ros::NodeHandle __nh;
        ros::Publisher __line_pub;
        ros::Publisher __points_pub;

        vector<geometry_msgs::Point> __points;
};

BezierCurve::BezierCurve(){
    __line_pub = __nh.advertise<nav_msgs::Path>("/bezier_curve", 1, true);
    __points_pub = __nh.advertise<sensor_msgs::PointCloud2>("/bezier_keypoints", 1, true);

    ros::Duration(1.0).sleep();  

    // // 生成一组随机数
    // random_device rd;
    // mt19937 gen(rd());
    // uniform_real_distribution<> dis_x(-5.0, 5.0);
    // uniform_real_distribution<> dis_y(-5.0, 5.0);
    // cout << "生成随机数据" << endl;
    // for (int i = 0; i < 5; ++i) {
    //     geometry_msgs::Point point;
    //     point.y = dis_y(gen);
    //     point.x = dis_x(gen) * point.y;
    //     cout << "--> [" << i << "] data: x= " << point.x << ", y=" << point.y << endl;
    //     __points.emplace_back(point);
    // }

    geometry_msgs::Point point;
    point.x = 0.0;
    point.y = 0.0;
    __points.emplace_back(point);
    point.x = 2.0;
    point.y = 2.0;
    __points.emplace_back(point);
    point.x = 4.0;
    point.y = 0.0;
    __points.emplace_back(point);
    // point.x = 3.0;
    // point.y = -3.0;
    // __points.emplace_back(point);
    // point.x = 5.0;
    // point.y = 2.0;
    // __points.emplace_back(point);
    // point.x = 6.0;
    // point.y = 0.0;
    // __points.emplace_back(point);

    // 发布点云数据
    PublishPointCloud2(__points);

    // 生成贝塞尔曲线
    vector<geometry_msgs::Point> curve_points;
    float step = 0.1;
    float dx = __points.front().x - __points.back().x;
    float dy = __points.front().y - __points.back().y;
    float dis = hypot(dx, dy);
    int nums = int(dis / step);
    for (int i = 0.0; i < nums; i++) {
        double t = (double)i / (double)(nums -1);
        geometry_msgs::Point point = BezierPoint(t, __points);
        cout << "--> t= " << t << ", x=" << point.x << ", y=" << point.y << endl;
        curve_points.emplace_back(point);
    }
    PublishLine(curve_points);
}

// 
int BezierCurve::Comb(int n, int r){
    if ((r == 0) || (r == n))
        return 1;
    return Comb(n - 1, r - 1) + Comb(n - 1, r);
}

// 计算贝塞尔曲线关键点
geometry_msgs::Point BezierCurve::BezierPoint(double t, vector<geometry_msgs::Point> points){
    geometry_msgs::Point new_point;
    size_t n = points.size() - 1;
    for(size_t i = 0; i < n + 1; i++){
        new_point.x += Comb(n, i) * std::pow(t, i) * std::pow(1 - t, n - i) * points[i].x;
        new_point.y += Comb(n, i) * std::pow(t, i) * std::pow(1 - t, n - i) * points[i].y;
    }
    return new_point;
}


// 发布点云数据
void BezierCurve::PublishPointCloud2(vector<geometry_msgs::Point> points){
    sensor_msgs::PointCloud2 cloud_msg;
    cloud_msg.header.frame_id = "map";
    cloud_msg.header.stamp = ros::Time::now();
    cloud_msg.height = 1;
    cloud_msg.width = points.size();
    cloud_msg.is_dense = false;

    sensor_msgs::PointCloud2Modifier modifier(cloud_msg);
    modifier.setPointCloud2FieldsByString(1, "xyz");
    modifier.resize(points.size());

    // 使用迭代器填充点云消息的数据
    sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_msg, "y");
    sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_msg, "z");
    for(const auto& point : points) {
        *iter_x = point.x;
        *iter_y = point.y;
        *iter_z = point.z;

        ++iter_x;
        ++iter_y;
        ++iter_z;
    }
    __points_pub.publish(cloud_msg);
}

// 发布直线数据
void BezierCurve::PublishLine(vector<geometry_msgs::Point> points){
    nav_msgs::Path path_msg;
    path_msg.header.frame_id = "map";
    path_msg.header.stamp = ros::Time::now();
    for(auto point : points){
        geometry_msgs::PoseStamped pose_stamp;
        pose_stamp.pose.position = point;
        path_msg.poses.emplace_back(pose_stamp);
    }
    __line_pub.publish(path_msg);
}


/******************************************************
 * 主函数 * 
 ******************************************************/
int main(int argc, char** argv){
    ros::init(argc, argv, "test_bezier_curve");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
	
    ROS_INFO("test_bezier_curve initializating......");
    ros::NodeHandle nh;
    BezierCurve bezier_curve;
    ros::spin();
    return 0;
}